#include "clampcarry.h"
#include "car_control.h"
#include "K230data_receiver.h"
#include "line_patrol.h"

extern int32_t x, y, color_mode;
uint8_t cnt2 = 0;
void clamp_open(void)
{
  __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 200);
}

void clamp_close(void)
{
  __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 150);
}

void clamp_arrivecentre(void)
{
  __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 106);
}

void clamp_turntable(void)
{
  __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 230);
}

void turntable(uint8_t num)
{
  if(num == 1)
  {
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 55);
  }
  else if(num == 2)
  {
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 143);
  }
  else if(num == 3)
  {
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 229);
  }
}

void slide_rail_up(void)
{
  Emm_V5_Pos_Control(5, 0, 100, 10, 10800, 0, 0); 
}

void slide_rail_down(void)
{
  Emm_V5_Pos_Control(5, 1, 100, 10, 10800, 0, 0); 
}

void clamp_arrive_turntable(void)
{
  Emm_V5_Pos_Control(5, 1, 100, 8, 4000, 0, 0); 
}

void clamp_leave_turntable(void)
{
  Emm_V5_Pos_Control(5, 0, 100, 8, 4000, 0, 0); 
  
}

void carry(uint8_t num)
{
  clamp_open();
  clamp_arrivecentre();
  HAL_Delay(1000);
  slide_rail_down();
  HAL_Delay(2000);
  clamp_open();
  HAL_Delay(500);
  clamp_close();
  HAL_Delay(500);
  slide_rail_up();
  HAL_Delay(2000);
  turntable(num);
  HAL_Delay(500);
  clamp_turntable();
  HAL_Delay(1000);
  clamp_arrive_turntable();
  HAL_Delay(1000);
  __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 158);
  HAL_Delay(1000);
  clamp_leave_turntable();
  HAL_Delay(2000);
  clamp_arrivecentre();
  HAL_Delay(1000);
}

void put(uint8_t num)
{
  clamp_turntable();
  __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 158);
  turntable(num);
  HAL_Delay(1000);
  clamp_arrive_turntable();
  HAL_Delay(1500);
  clamp_close();
  HAL_Delay(500);
  clamp_leave_turntable();
  HAL_Delay(2000);
  clamp_arrivecentre();
  HAL_Delay(1000);
  slide_rail_down();
  HAL_Delay(3000);
  clamp_open();
  HAL_Delay(500);
  slide_rail_up();
  HAL_Delay(3000);
}

uint8_t clamp_num3(void)
{
  uint8_t blackline_flag = 0;
  uint8_t movecentre_flag = 0;
  while(1)
  {
    blackline_flag = meet_blackline();
    if(blackline_flag)
    {
      car_stop();
      HAL_Delay(500);
      car_ahead(pos,30,3000);
      HAL_Delay(1500);
      car_stop();
//      Send_Mode(1);
      HAL_Delay(3500);
      
      movecentre_flag = move_to_colorcentre2();
      if(movecentre_flag)
      {
        carry(movecentre_flag);
        movecentre_flag = 0;
      }
      car_ahead(pos,30,3000);
//      Send_Mode(2);
      HAL_Delay(1300);
      car_stop();
      HAL_Delay(1500);
      movecentre_flag = move_to_colorcentre2();
      if(movecentre_flag)
      {
        carry(movecentre_flag);
        movecentre_flag = 0;
      }
      car_ahead(pos,30,3000);
//      Send_Mode(3);
      HAL_Delay(1500);
      car_stop();
      HAL_Delay(1500);
      movecentre_flag = move_to_colorcentre2();
      if(movecentre_flag)
      {
        carry(movecentre_flag);
        movecentre_flag = 0;
        Send_Mode(6);
        return 1;
      }
    }
  }
}

uint8_t put_num3(uint8_t flag)
{
  uint8_t movecentre_flag = 0;
  while(1)
  {
    if(flag)
    {
      car_stop();
      HAL_Delay(500);
      car_ahead(pos,30,3000);
      HAL_Delay(1500);
      car_stop();
      while(1)
      {
        Send_Mode(6);
        cnt2++;
        HAL_Delay(500);
        if(color_mode == 6 || cnt2 > 1)
        {
          cnt2 = 0;
          break;
        }
      }
      HAL_Delay(3000);
      movecentre_flag = move_to_colorcentre1();
      if(movecentre_flag)
      {
        movecentre_flag = 0;
        put(3);
      }
      car_ahead(pos,30,3000);
      HAL_Delay(1500);
      car_stop();
      while(1)
      {
        Send_Mode(4);
        cnt2++;
        HAL_Delay(500);
        if(color_mode == 4 || cnt2 > 1)
        {
          cnt2 = 0;
          break;
        }
      }
      HAL_Delay(3000);
      movecentre_flag = move_to_colorcentre1();
      if(movecentre_flag)
      {
        movecentre_flag = 0;
        put(1);
      }
      car_ahead(pos,30,3000);
      HAL_Delay(1500);
      car_stop();
      while(1)
      {
        Send_Mode(5);
        cnt2++;
        HAL_Delay(500);
        if(color_mode == 5|| cnt2 > 1)
        {
          cnt2 = 0;
          break;
        }
      }
      HAL_Delay(3000);
      movecentre_flag = move_to_colorcentre1();
      if(movecentre_flag)
      {
        movecentre_flag = 0;
        put(2);
        return 1;
      }
    }
  }
}